#include "volume3dcritical.h"

using namespace PBVP;

Volume3DCritical::Volume3DCritical(Reg3DVolumeData* pData)
{
	m_pData = pData;
	m_pData->getDimension(m_Dim);
	m_pData->getOrigin(m_Orig);
	m_pData->getSpan(m_Span);
}

int Volume3DCritical::getTetraIndex(int idx1[3], int idx2[3])
{
	if (!areNeighbors(idx1, idx2))
	{
		printf("non-neighboring verts, special case....\n");
		int k, dx[3], nidx[3];
		for (k = 0; k < 3; k++)
		{
			dx[k] = idx2[k] - idx1[k];
		}
		bool flag = false;
		int angle = 0;
		float val = m_pData->getValue(m_pData->index2vert(idx1));
		for (k = 0; k < 14; k++)
		{
			nidx[0] = idx1[0] + Tetra::six_simplex_neighbors[k][0];
			nidx[1] = idx1[1] + Tetra::six_simplex_neighbors[k][1];
			nidx[2] = idx1[2] + Tetra::six_simplex_neighbors[k][2];
			if (m_pData->getValue(m_pData->index2vert(nidx)) > val)
			{
				if ((dx[0]*Tetra::six_simplex_neighbors[k][0]+dx[1]*Tetra::six_simplex_neighbors[k][1]
				+dx[2]*Tetra::six_simplex_neighbors[k][2]) < angle || (!flag))
				{
					idx2[0] = nidx[0];
					idx2[1] = nidx[1];
					idx2[2] = nidx[2];
					angle = dx[0]*Tetra::six_simplex_neighbors[k][0]+dx[1]*Tetra::six_simplex_neighbors[k][1]+dx[2]*Tetra::six_simplex_neighbors[k][2];
					flag = true;
				}
			}
		}
		assert(flag);
	}

	int i, j, cidx[3];
	cidx[0] = std::min(idx1[0], idx2[0]);
	cidx[1] = std::min(idx1[1], idx2[1]);
	cidx[2] = std::min(idx1[2], idx2[2]);

	for (i = 0; i < 3; i++) cidx[i] = std::min(cidx[i], (int)m_Dim[i]-2);

	int lv1 = (idx1[0]-cidx[0]) + ((idx1[1]-cidx[1]) << 1) + ((idx1[2]-cidx[2]) << 2);
	int lv2 = (idx2[0]-cidx[0]) + ((idx2[1]-cidx[1]) << 1) + ((idx2[2]-cidx[2]) << 2);

	for (i = 0; i < 6; i++)
	{
		bool flag1, flag2;
		flag1 = flag2 = false;
		for (j = 0; j < 4; j++)
		{
			if (lv1 == Tetra::six_tetra_verts[i][j]) flag1 = true;
			if (lv2 == Tetra::six_tetra_verts[i][j]) flag2 = true;
		}
		if (flag1 && flag2)	break;
	}

	return 6*m_pData->index2cell(cidx[0], cidx[1], cidx[2])+i;
}

void Volume3DCritical::sortVerts(float* &x0, float* &x1, float* &x2, float* &x3, float v[4], float f[4])
{
	float *_t, t;
	if (v[3] < v[2])
	{
		_t = x3; x3 = x2; x2 = _t;
		t = v[3]; v[3] = v[2]; v[2] = t;
		t = f[3]; f[3] = f[2]; f[2] = t;
	}
	if (v[2] < v[1])
	{
		_t = x2; x2 = x1; x1 = _t;
		t = v[2]; v[2] = v[1]; v[1] = t;
		t = f[2]; f[2] = f[1]; f[1] = t;
	}
	if (v[1] < v[0])
	{
		_t = x1; x1 = x0; x0 = _t;
		t = v[1]; v[1] = v[0]; v[0] = t;
		t = f[1]; f[1] = f[0]; f[0] = t;
	}
	if (v[3] < v[2])
	{
		_t = x3; x3 = x2; x2 = _t;
		t = v[3]; v[3] = v[2]; v[2] = t;
		t = f[3]; f[3] = f[2]; f[2] = t;
	}
	if (v[2] < v[1])
	{
		_t = x2; x2 = x1; x1 = _t;
		t = v[2]; v[2] = v[1]; v[1] = t;
		t = f[2]; f[2] = f[1]; f[1] = t;
	}
	if (v[3] < v[2])
	{
		_t = x3; x3 = x2; x2 = _t;
		t = v[3]; v[3] = v[2]; v[2] = t;
		t = f[3]; f[3] = f[2]; f[2] = t;
	}

	//float epsilon = std::min((p_fun->getFuncMax()-p_fun->getFuncMin())/(p_fun->getVertCount()*100), (v[3]-v[0])/400);
	//epsilon = 0.1*std::max(epsilon, (v[3]-v[0])/4000);
	float epsilon = std::max(1e-5f, (v[3]-v[1])/4000);
	if (v[1] <= v[0]+epsilon) v[1] = v[0] + epsilon;
	if (v[2] <= v[1]+epsilon) v[2] = v[1] + epsilon;
	if (v[3] <= v[2]+epsilon) v[3] = v[2] + epsilon;
}

int Volume3DCritical::getMaxValNeighbor(int vid)
{
	int i, idx[3], nidx[3], v2;
	float lcl_max;
	m_pData->vert2index(vid, idx);
	v2 = -1;
	for (i = 0; i < 14; i++)
	{
		nidx[0] = idx[0] + Tetra::six_simplex_neighbors[i][0];
		nidx[1] = idx[1] + Tetra::six_simplex_neighbors[i][1];
		nidx[2] = idx[2] + Tetra::six_simplex_neighbors[i][2];

		if (isValidVertex(nidx))
		{
			int nid = m_pData->index2vert(nidx);
			if (v2 < 0 || getCPValue(nid) > lcl_max )
			{
				v2 = nid;
				lcl_max = getCPValue(nid);
			}
		}
	}
	return v2;
}

int Volume3DCritical::getMinValNeighbor(int vid)
{
	int i, idx[3], nidx[3], v2;
	float lcl_min;

	m_pData->vert2index(vid, idx);
	v2 = -1;
	for (i = 0; i < 14; i++)
	{
		nidx[0] = idx[0] + Tetra::six_simplex_neighbors[i][0];
		nidx[1] = idx[1] + Tetra::six_simplex_neighbors[i][1];
		nidx[2] = idx[2] + Tetra::six_simplex_neighbors[i][2];
		if (isValidVertex(nidx))
		{
			int nid = m_pData->index2vert(nidx);
			if (v2 < 0 || getCPValue(nid) < lcl_min )
			{
				v2 = nid;
				lcl_min = getCPValue(nid);
			}
		}
	}
	return v2;
}

void Volume3DCritical::getTetraProperty(int tid, float pot[4], int np)
{
	assert(np <= getNumofProperties());
	int cid = tid / 6;
	int tet = tid % 6;
	int idx[3];
	m_pData->cell2index(cid, idx);

	pot[0] = getPropertyValue(idx[0]+Tetra::six_tetra_index[tet][0][0], idx[1]+Tetra::six_tetra_index[tet][0][1], idx[2]+Tetra::six_tetra_index[tet][0][2], np);
	pot[1] = getPropertyValue(idx[0]+Tetra::six_tetra_index[tet][1][0], idx[1]+Tetra::six_tetra_index[tet][1][1], idx[2]+Tetra::six_tetra_index[tet][1][2], np);
	pot[2] = getPropertyValue(idx[0]+Tetra::six_tetra_index[tet][2][0], idx[1]+Tetra::six_tetra_index[tet][2][1], idx[2]+Tetra::six_tetra_index[tet][2][2], np);
	pot[3] = getPropertyValue(idx[0]+Tetra::six_tetra_index[tet][3][0], idx[1]+Tetra::six_tetra_index[tet][3][1], idx[2]+Tetra::six_tetra_index[tet][3][2], np);	
}



int Volume3DCritical::getCutEdge(float cut_val, int &v1)
{
	float f;

	int v2 = -1;
	if(m_pData->getValue(v1) == cut_val)	// cut on the vertex v1
	{
		if ((v2 = getMaxValNeighbor(v1)) < 0)
		{
			v2 = getMinValNeighbor(v1);
		}
		return v2;
	}

	if (m_pData->getValue(v1) < cut_val)
	{
		v2 = v1;
		while (m_pData->getValue(v2) < cut_val)
		{
			//search for v1's neighbors that has the max value > f(v1)
			v1 = v2;
			v2 = getMaxValNeighbor(v1);
		}
	} else if ((f = m_pData->getValue(v1)) > cut_val)
	{
		v2 = v1;
		while ((f = m_pData->getValue(v2)) > cut_val)
		{
			v1 = v2;
			v2 = getMinValNeighbor(v1);
		}
	}
	return v2;
}

int Volume3DCritical::getCutEdge(float cut_val, float range[2], int &v1) {
	int v2 = -1;

	if (getCPValue(v1) == cut_val)
	{
		assert(getCPValue(v1) == range[0] || getCPValue(v1) == range[1]);
		if (getCPValue(v1) <= range[0])
		{
			v2 = getMaxValNeighbor(v1);
		} else if (getCPValue(v1) >= range[1])
		{
			v2 = getMinValNeighbor(v1);
		}
		return v2;
	}

	if (getCPValue(v1) < cut_val)
	{
		if (getCPValue(v1) >= range[0])
		{
			v2 = getMaxValNeighbor(v1);
			return v2;
		}
		v2 = v1;
		while (getCPValue(v2) <= cut_val)
		{
			//search for v1's neighbors that has the max value > f(v1)
			v1 = v2;
			v2 = getMaxValNeighbor(v1);
		}
	} else if (getCPValue(v1) > cut_val)
	{
		if (getCPValue(v1) <= range[1])
		{
			v2 = getMinValNeighbor(v1);
			return v2;
		}
		v2 = v1;
		while (getCPValue(v2) > cut_val)
		{
			v1 = v2;
			v2 = getMinValNeighbor(v1);
		}
	}
	return v2;
}

/****************************************************************/
/* Carbo Index 													*/
/****************************************************************/

/* float PBVP::CarboIndex(const ConTreeConstructorReg3D& reg1, float **R1, float *C1,
const ConTreeConstructorReg3D& reg2, float **R2, float *C2)
{
int i, j, k;
// A = R2*R1^T
float **A = matrix(0, 2, 0, 2);
for (i = 0; i < 3; i++)
{
for (j = 0; j < 3; j++)
{
A[i][j] = 0;
for (k = 0; k < 3; k++)
{
A[i][j] += R2[i][k]*R1[j][k];
}
}
}

// compute |f1| and |f2|
float cv1 = reg1.m_Span[0]*reg1.m_Span[1]*reg1.m_Span[2];
float cv2 = reg2.m_Span[0]*reg2.m_Span[1]*reg2.m_Span[2];
float x = 0;
for (k = 0; k < reg1.m_Dim[2]; k++)
{
for (j = 0; j < reg1.m_Dim[1]; j++)
{
for (i = 0; i < reg1.m_Dim[0]; i++)
{
x += reg1.getPropertyValue(i, j, k) * reg1.getPropertyValue(i, j, k);
}
}
}
float norm_f1 = (float) sqrt(x*cv1);
x = 0;
for (k = 0; k < reg2.m_Dim[2]; k++)
{
for (j = 0; j < reg2.m_Dim[1]; j++)
{
for (i = 0; i < reg2.m_Dim[0]; i++)
{
x += reg2.getPropertyValue(i, j, k) * reg2.getPropertyValue(i, j, k);
}
}
}
float norm_f2 = (float) sqrt(x*cv2);
printf("f1 norm = %.8f, f2 norm = %.8f\n", norm_f1, norm_f2);

// compute dot product <f1, f2>
x = 0;
float coord[3], pnt[3];
for (k = 0; k < reg1.m_Dim[2]; k++)
{
for (j = 0; j < reg1.m_Dim[1]; j++)
{
for (i = 0; i < reg1.m_Dim[0]; i++)
{
coord[0] = reg1.m_Orig[0] + i*reg1.m_Span[0] - C1[0];
coord[1] = reg1.m_Orig[1] + j*reg1.m_Span[1] - C1[1];
coord[2] = reg1.m_Orig[2] + k*reg1.m_Span[2] - C1[2];
float f1 = reg1.getPropertyValue(i, j, k);
// float f1 = reg1.getCPValue(i, j, k, 1);
for (int l = 0; l < 3; l++)
{
pnt[l] = A[l][0]*coord[0]+A[l][1]*coord[1]+A[l][2]*coord[2] + C2[l];
}
float f2 = reg2.getPropertyValue(pnt);
// float f2 = reg2.getCPValue(pnt, 1);
//printf("p1 = (%f %f %f) f1 = %f -> p2 = (%f %f %f) f2 = %f\n", coord[0]+C1[0], coord[1]+C1[1],
//		coord[2]+C1[2], f1, pnt[0], pnt[1], pnt[2], f2);
x += f1*f2;
}
}
}
x *= cv1;
float carbo = x / (norm_f1*norm_f2);
printf("Carbo index: %.8f\n", carbo);
free_matrix(A, 0, 2, 0, 2);
return carbo;
}*/

/* float ConTreeConstructorReg3D::calcVolume(float range[2], int start_tid, float &fint)
{
float volum = 0;
int i, idx[3];
float face_rang[2];

char *touched = new char[6*(m_Dim[0]-1)*(m_Dim[1]-1)*(m_Dim[2]-1)];
memset(touched, 0, 6*(m_Dim[0]-1)*(m_Dim[1]-1)*(m_Dim[2]-1));
std::deque<int> tetque;
touched[start_tid] = 1;
tetque.push_back(start_tid);

fint = 0;
while (!tetque.empty())
{
int tid = tetque.front();
tetque.pop_front();
int cid = tid / 6;
int tet = tid %6;
cell2Index(cid, idx);
float vals[4];
vals[0] = getCPValue(index2ID(idx[0]+six_tetra_index[tet][0][0], idx[1]+six_tetra_index[tet][0][1], idx[2]+six_tetra_index[tet][0][2]));
vals[1] = getCPValue(index2ID(idx[0]+six_tetra_index[tet][1][0], idx[1]+six_tetra_index[tet][1][1], idx[2]+six_tetra_index[tet][1][2]));
vals[2] = getCPValue(index2ID(idx[0]+six_tetra_index[tet][2][0], idx[1]+six_tetra_index[tet][2][1], idx[2]+six_tetra_index[tet][2][2]));
vals[3] = getCPValue(index2ID(idx[0]+six_tetra_index[tet][3][0], idx[1]+six_tetra_index[tet][3][1], idx[2]+six_tetra_index[tet][3][2]));
float x0[3], x1[3], x2[3], x3[3];
for (i = 0; i < 3; i++)
{
x0[i] = m_Orig[i]+(idx[i] + six_tetra_index[tet][0][i])*m_Span[i];
x1[i] = m_Orig[i]+(idx[i] + six_tetra_index[tet][1][i])*m_Span[i];
x2[i] = m_Orig[i]+(idx[i] + six_tetra_index[tet][2][i])*m_Span[i];
x3[i] = m_Orig[i]+(idx[i] + six_tetra_index[tet][3][i])*m_Span[i];
}
// adjust values if they are equal
float tmax = std::max(std::max(vals[0], vals[1]), std::max(vals[2], vals[3]));
float tmin = std::min(std::min(vals[0], vals[1]), std::min(vals[2], vals[3]));
//epsilon = std::min((p_fun->getFuncMax()-p_fun->getFuncMin())/(p_fun->getVertCount()*100), (tmax-tmin)/400);
//printf("fmax: %f %f, fmin: %f %f, esp: %f\n", p_fun->getFuncMax(), tmax, p_fun->getFuncMin(), tmin, epsilon);
float delta = subVolume(x0, x1, x2, x3, vals, range);
volum = volum + delta;
if (p_Potential)
{
float f[4];
f[0] = p_Potential->getValue(index2ID(idx[0]+six_tetra_index[tet][0][0], idx[1]+six_tetra_index[tet][0][1], idx[2]+six_tetra_index[tet][0][2]));
f[1] = p_Potential->getValue(index2ID(idx[0]+six_tetra_index[tet][1][0], idx[1]+six_tetra_index[tet][1][1], idx[2]+six_tetra_index[tet][1][2]));
f[2] = p_Potential->getValue(index2ID(idx[0]+six_tetra_index[tet][2][0], idx[1]+six_tetra_index[tet][2][1], idx[2]+six_tetra_index[tet][2][2]));
f[3] = p_Potential->getValue(index2ID(idx[0]+six_tetra_index[tet][3][0], idx[1]+six_tetra_index[tet][3][1], idx[2]+six_tetra_index[tet][3][2]));    
float tint = subFuncInteg(x0, x1, x2, x3, vals, f, range);
fint += tint;
//printf("dv: %f, df: %f; vol = %f, fint = %f\n", delta, tint, volum, fint);
}
#ifdef DEBUG_VOLVOL
if (delta < 0) {
printf("tetra %d, range (%f, %f), vals = (%f, %f, %f, %f), vol = %f, delta = %f\n", 
tid, range[0], range[1], vals[0], vals[1], vals[2], vals[3], volum, delta);
//printf("x0 = (%f, %f, %f)\n", x0[0], x0[1], x0[2]);
//printf("x1 = (%f, %f, %f)\n", x1[0], x1[1], x1[2]);
//printf("x2 = (%f, %f, %f)\n", x2[0], x2[1], x2[2]);
//printf("x3 = (%f, %f, %f)\n", x3[0], x3[1], x3[2]);
}
#endif
for (i = 0; i < 4; i++)
{
face_rang[0] = std::min(vals[(i+1)%4], std::min(vals[(i+2)%4], vals[(i+3)%4]));
face_rang[1] = std::max(vals[(i+1)%4], std::max(vals[(i+2)%4], vals[(i+3)%4]));
if (!disjointRange(range, face_rang))
{
int ngb_tid = tetNeighbor(tid, i);
if (ngb_tid >= 0 && !touched[ngb_tid])
{
touched[ngb_tid] = 1;
tetque.push_back(ngb_tid);
}
}
}
}

delete[] touched;

double vol_norm = 1.0 / getVolume();
fint = float (vol_norm * fint);
return float (volum * vol_norm);
//return volum;
}

#define N_MOM 20

VolMoments ConTreeConstructorReg3D::calcMoments(float range[2], int start_tid) 
{
VolMoments mom;

double volum = 0;
int i, idx[3];
float face_rang[2];

char *touched = new char[6*(m_Dim[0]-1)*(m_Dim[1]-1)*(m_Dim[2]-1)];
memset(touched, 0, 6*(m_Dim[0]-1)*(m_Dim[1]-1)*(m_Dim[2]-1));
std::deque<int> tetque;
touched[start_tid] = 1;
tetque.push_back(start_tid);

while (!tetque.empty())
{
int tid = tetque.front();
tetque.pop_front();
int cid = tid / 6;
int tet = tid %6;
cell2Index(cid, idx);
float vals[4], pot[4] = {0, 0, 0, 0};
vals[0] = getCPValue(index2ID(idx[0]+six_tetra_index[tet][0][0], idx[1]+six_tetra_index[tet][0][1], idx[2]+six_tetra_index[tet][0][2]));
vals[1] = getCPValue(index2ID(idx[0]+six_tetra_index[tet][1][0], idx[1]+six_tetra_index[tet][1][1], idx[2]+six_tetra_index[tet][1][2]));
vals[2] = getCPValue(index2ID(idx[0]+six_tetra_index[tet][2][0], idx[1]+six_tetra_index[tet][2][1], idx[2]+six_tetra_index[tet][2][2]));
vals[3] = getCPValue(index2ID(idx[0]+six_tetra_index[tet][3][0], idx[1]+six_tetra_index[tet][3][1], idx[2]+six_tetra_index[tet][3][2]));
if (p_Potential)
{
pot[0] = p_Potential->getValue(index2ID(idx[0]+six_tetra_index[tet][0][0], idx[1]+six_tetra_index[tet][0][1], idx[2]+six_tetra_index[tet][0][2]));
pot[1] = p_Potential->getValue(index2ID(idx[0]+six_tetra_index[tet][1][0], idx[1]+six_tetra_index[tet][1][1], idx[2]+six_tetra_index[tet][1][2]));
pot[2] = p_Potential->getValue(index2ID(idx[0]+six_tetra_index[tet][2][0], idx[1]+six_tetra_index[tet][2][1], idx[2]+six_tetra_index[tet][2][2]));
pot[3] = p_Potential->getValue(index2ID(idx[0]+six_tetra_index[tet][3][0], idx[1]+six_tetra_index[tet][3][1], idx[2]+six_tetra_index[tet][3][2]));
}
float x0[3], x1[3], x2[3], x3[3];
for (i = 0; i < 3; i++)
{
x0[i] = m_Orig[i]+(idx[i] + six_tetra_index[tet][0][i])*m_Span[i];
x1[i] = m_Orig[i]+(idx[i] + six_tetra_index[tet][1][i])*m_Span[i];
x2[i] = m_Orig[i]+(idx[i] + six_tetra_index[tet][2][i])*m_Span[i];
x3[i] = m_Orig[i]+(idx[i] + six_tetra_index[tet][3][i])*m_Span[i];
}

float f1[N_MOM], f2[N_MOM], f3[N_MOM], f4[N_MOM], out[N_MOM];
f1[0] = f2[0] = f3[0] = f4[0] = 1;
f1[1] = x0[0]; f2[1] = x1[0]; f3[1] = x2[0]; f4[1] = x3[0];
f1[2] = x0[1]; f2[2] = x1[1]; f3[2] = x2[1]; f4[2] = x3[1];
f1[3] = x0[2]; f2[3] = x1[2]; f3[3] = x2[2]; f4[3] = x3[2];
f1[4] = x0[0]*x0[0]; f2[4] = x1[0]*x1[0]; f3[4] = x2[0]*x2[0]; f4[4] = x3[0]*x3[0];
f1[5] = x0[1]*x0[1]; f2[5] = x1[1]*x1[1]; f3[5] = x2[1]*x2[1]; f4[5] = x3[1]*x3[1];
f1[6] = x0[2]*x0[2]; f2[6] = x1[2]*x1[2]; f3[6] = x2[2]*x2[2]; f4[6] = x3[2]*x3[2];
f1[7] = x0[0]*x0[1]; f2[7] = x1[0]*x1[1]; f3[7] = x2[0]*x2[1]; f4[7] = x3[0]*x3[1];
f1[8] = x0[0]*x0[2]; f2[8] = x1[0]*x1[2]; f3[8] = x2[0]*x2[2]; f4[8] = x3[0]*x3[2];
f1[9] = x0[1]*x0[2]; f2[9] = x1[1]*x1[2]; f3[9] = x2[1]*x2[2]; f4[9] = x3[1]*x3[2];                   
f1[10] = pot[0]*x0[0]; f2[10] = pot[1]*x1[0]; f3[10] = pot[2]*x2[0]; f4[10] = pot[3]*x3[0];
f1[11] = pot[0]*x0[1]; f2[11] = pot[1]*x1[1]; f3[11] = pot[2]*x2[1]; f4[11] = pot[3]*x3[1];
f1[12] = pot[0]*x0[2]; f2[12] = pot[1]*x1[2]; f3[12] = pot[2]*x2[2]; f4[12] = pot[3]*x3[2];
f1[13] = pot[0]*x0[0]*x0[0]; f2[13] = pot[1]*x1[0]*x1[0]; f3[13] = pot[2]*x2[0]*x2[0]; f4[13] = pot[3]*x3[0]*x3[0];
f1[14] = pot[0]*x0[1]*x0[1]; f2[14] = pot[1]*x1[1]*x1[1]; f3[14] = pot[2]*x2[1]*x2[1]; f4[14] = pot[3]*x3[1]*x3[1];
f1[15] = pot[0]*x0[2]*x0[2]; f2[15] = pot[1]*x1[2]*x1[2]; f3[15] = pot[2]*x2[2]*x2[2]; f4[15] = pot[3]*x3[2]*x3[2];
f1[16] = pot[0]*x0[0]*x0[1]; f2[16] = pot[1]*x1[0]*x1[1]; f3[16] = pot[2]*x2[0]*x2[1]; f4[16] = pot[3]*x3[0]*x3[1];
f1[17] = pot[0]*x0[0]*x0[2]; f2[17] = pot[1]*x1[0]*x1[2]; f3[17] = pot[2]*x2[0]*x2[2]; f4[17] = pot[3]*x3[0]*x3[2];
f1[18] = pot[0]*x0[1]*x0[2]; f2[18] = pot[1]*x1[1]*x1[2]; f3[18] = pot[2]*x2[1]*x2[2]; f4[18] = pot[3]*x3[1]*x3[2];                       
f1[19] = pot[0]; f2[19] = pot[1]; f3[19] = pot[2]; f4[19] = pot[3];

subFuncIntegMul(x0, x1, x2, x3, vals, f1, f2, f3, f4, out, N_MOM, range);
if(!(out[0] <= 1)) {
printf("vals = [%f %f %f %f], range = [%f %f]\n", vals[0], vals[1],
vals[2], vals[3], range[0], range[1]);
exit(1);
//printf("vol delta = %f\n", out[0]);
printf("vals = [%f %f %f %f], pot = {%f %f %f %f}, vol = %f , fint = %f\n",
vals[0], vals[1], vals[2], vals[3], pot[0], pot[1], pot[2], pot[3],
out[0], out[19]);
}

mom.vol += out[0];
mom.Px += out[1];
mom.Py += out[2];
mom.Pz += out[3];
mom.Pxx += out[4];
mom.Pyy += out[5];
mom.Pzz += out[6];
mom.Pxy += out[7];
mom.Pxz += out[8];
mom.Pyz += out[9];
mom.Fx += out[10];
mom.Fy += out[11];
mom.Fz += out[12];
mom.Fxx += out[13];
mom.Fyy += out[14];
mom.Fzz += out[15];
mom.Fxy += out[16];
mom.Fxz += out[17];
mom.Fyz += out[18];
mom.Fint += out[19];

if (mom.Fmin > std::min(std::min(pot[0], pot[1]), std::min(pot[2], pot[3]))) mom.Fmin = std::min(std::min(pot[0], pot[1]), std::min(pot[2], pot[3]));
if (mom.Fmax < std::max(std::max(pot[0], pot[1]), std::max(pot[2], pot[3]))) mom.Fmax = std::max(std::max(pot[0], pot[1]), std::max(pot[2], pot[3]));

for (i = 0; i < 4; i++)
{
face_rang[0] = std::min(vals[(i+1)%4], std::min(vals[(i+2)%4], vals[(i+3)%4]));
face_rang[1] = std::max(vals[(i+1)%4], std::max(vals[(i+2)%4], vals[(i+3)%4]));
if (!disjointRange(range, face_rang))
{
int ngb_tid = tetNeighbor(tid, i);
if (ngb_tid >= 0 && !touched[ngb_tid])
{
touched[ngb_tid] = 1;
tetque.push_back(ngb_tid);
}
}
}
}

delete[] touched;
//mom.print();
return mom;
}
*/